/*
 * i2c_as5600.c
 *
 *  Created on: Apr 16, 2024
 *      Author: Yiwen Zhu
 */
#include "i2c_as5600_dma.h"
#include "Motor_Control_Init.h"

//static float angle_prev=0;
//static int full_rotations=0; // full rotation tracking;
//static float angle_d;				//GetAngle_Without_Track()的返回值
//static float angle_cd;				//GetAngle()的返回值

#define cpr (float)(2.0f*PI)
#define Slave_Addr                0x36<<1//设备从地址
#define Write_Bit                 0	   //写标记
#define Read_Bit                  1    //读标记
#define Angle_Hight_Register_Addr 0x0C //寄存器高位地址
#define Angle_Low_Register_Addr   0x0D //寄存器低位地址

#define DATA_SIZE	(2)

//发送单字节时序
void AS5600_Write_Reg_dma(uint16_t reg, uint8_t *value)
{
	HAL_I2C_Mem_Write(&hi2c3, Slave_Addr, reg, I2C_MEMADD_SIZE_8BIT, value, 1, 50);
}


//发送多字节时序
void AS5600_Write_Regs_dma(uint16_t reg, uint8_t *value, uint8_t len)
{
	HAL_I2C_Mem_Write(&hi2c3, Slave_Addr, reg, I2C_MEMADD_SIZE_8BIT, value, len, 50);
}


//IIC读多字节
void AS5600_Read_Reg_dma(uint16_t reg, uint8_t* buf, uint8_t len)
{
	HAL_I2C_Mem_Read_DMA(&hi2c3, Slave_Addr, reg, I2C_MEMADD_SIZE_8BIT, buf, len);
}

// Send a dma read request
static uint8_t temp[DATA_SIZE]={0};
void as5600_dma_request(void){
	AS5600_Read_Reg_dma( Angle_Hight_Register_Addr, temp, DATA_SIZE);
}


//得到弧度制的角度，范围在0-6.28

float GetAngle_Without_Track_DMA(uint16_t NP)
{
	int16_t in_angle;
	float angle_d;

	in_angle = ((((int16_t)temp[0] <<8) | (temp[1])));
	G_Theta_Mech = in_angle-32768-32768;

	angle_d = (float)in_angle * (DUAL_PI_ELEC) / 4096.0f;
//	if(angle_d<0) angle_d += DUAL_PI_ELEC;
//angle_d为弧度制，范围在0-6.28
	return angle_d;
}



//
////得到弧度制的带圈数角度
//float GetAngle(void)
//{
//    float val = angle_d;
//    float d_angle = val - angle_prev;
//    //计算旋转的总圈数
//    //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。
//    if(fabs(d_angle) > (0.8f*2.0f*PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1;
//    angle_prev = val;
//
//		angle_cd = full_rotations * (2.0f*PI) + angle_prev;
//	return angle_cd;
////    return (float)full_rotations * 6.28318530718f + angle_prev;
//}
//
//
//void Track(void)
//{
//	GetAngle_Without_Track();
//	GetAngle();
//}
